Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Feature/filtered elevation layer #84

Open
wants to merge 38 commits into
base: master
Choose a base branch
from

Conversation

chisarie
Copy link

@chisarie chisarie commented Nov 5, 2018

A plugin for [costmap_2d]](http://wiki.ros.org/costmap_2d). It takes the elevation map as input and generates a layer of obstacles in the local costmap_2d.

Copy link
Contributor

@hogabrie hogabrie left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please have a look at our style guide.
Following the variable / function naming rules of costmap2d might make sense here, however make sure to use proper indents and formatting rules (s.a braces in short ifs). Also use doxygen style comments more consistently, look at the headers in elevation_mapping they are nicely documented.

@@ -0,0 +1,77 @@
# use c++11
set(CMAKE_CXX_COMPILER_ARG1 -std=c++11)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Put this in the build section of the cmake and use
add_definitions(-std=c++11)

)

## cpp-header files for installation
install(DIRECTORY include/
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think this should be:
install(DIRECTORY include/${PROJECT_NAME}/

cmake_minimum_required(VERSION 2.8.3)
project(elevation_layer)

find_package(Boost REQUIRED COMPONENTS thread)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You never actually use boost in the cmake.
See here for a how-to.

)

## declare a cpp library
add_library(elevation_layer
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

FYI you can use ${PROJECT_NAME} whenever you have elevation_layer.

@@ -0,0 +1,82 @@
//
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Follow this packages file naming:
e.g.

/*
 * ElevationMapping.cpp
 *
 *  Created on: Nov 12, 2013
 *      Author: Péter Fankhauser
 *	 Institute: ETH Zurich, ANYbotics
 */

}
bool track_unknown_space;
nh.param("track_unknown_space", track_unknown_space, layered_costmap_->isTrackingUnknown());
if (track_unknown_space)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Use curly braces(styleguide)! Or shorthand default_value = track_unknown_space ? NO_INFORMATION : FREE_SPACE;

std::lock_guard<std::mutex> lock(elevation_map_mutex_);
if (rolling_window_)
updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
if (!(enabled_ && elevation_map_received_))
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Makes more sense to do this before updating origin. Right?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think you still want your local costmap to move around with the robot even though the elevation_mapping is not received. But actually is kind of the same

if (!footprint_clearing_enabled_) return;
costmap_2d::transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_);

for (unsigned int i = 0; i < transformed_footprint_.size(); i++)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Use range based for:

for(auto&& point : transformed_footprint_) {
  touch(point.x, point.y, min_x, min_y, max_x, max_j);
}

}
if ( elevation_data(gridmap_index(0), gridmap_index(1)) > height_treshold_ ) // If point too high, it could be an obstacle
{
if (!has_edges_layer) {
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You could check this before even going into the for loop, since this is a property of the elevation_map. Also use a throttled message.


switch (combination_method_)
{
case 0: // Overwrite
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Use a string (that ideally maps to an enum) for combination_method_.

include_directories(include)
include_directories(
${catkin_INCLUDE_DIRS}
)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

you can have a single include_directories and list the elements

//

#ifndef ELEVATION_LAYER_H
#define ELEVATION_LAYER_H
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You can use #pragma once, a more modern way of doing this.

@@ -0,0 +1,32 @@
<?xml version="1.0"?>
<package>
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I would use package version 2 here, so you only have <depend> instead of <build_depend> and <run_depend>

<run_depend>costmap_2d</run_depend>
<run_depend>dynamic_reconfigure</run_depend>
<run_depend>grid_map_ros</run_depend>
<run_depend>filters</run_depend>
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

const std::string tf_prefix = tf::getPrefixParam(prefix_nh);

// get parameters from config file
if(!nh.param("elevation_topic", elevation_topic_, std::string("")))
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If you use param_io (in repo any_node), it will print warnings for you and you can shorten the code here.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I would not add this dependency in a package that could be used completely independently of our software stack.

<!-- <author email="echisari@anybotics.com">Eugenio Chisari</author> -->
<maintainer email="echisari@anybotics.com">Eugenio Chisari</maintainer>

<license>BSD</license>
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Are we open sourcing this plugin?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I dont know

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It's already open source, if we keep it in this repo 😉

protected:
std::string global_frame_; ///< @brief The global frame for the costmap
std::vector<boost::shared_ptr<message_filters::SubscriberBase> > elevation_subscribers_; ///< @brief Used for the observation message filters
dynamic_reconfigure::Server<elevation_layer::ElevationPluginConfig> *dsrv_;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If you use a smart pointer here, you do not need to care about memory handling (calling delete)


using costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::FREE_SPACE;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think that we soon need more than binary traversability information. Imagine you have two paths to reach the goal, one path on flat terrain and one path with ramps/steps etc. Both paths are traversable, but one has higher costs. In these cases the planner will need get these costs through the map. Therefore we will need traversability information between 0 and 1.

Unrelated to that, during testing we saw that noise could result to untraversable cells in the costmap. In these cases the planner failed to find a path. In my opinion it would make sense to have a threshold below which untraversable cells are not considered to be an obstacle.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

About the first point, yes its true, this one is the short term solution, in the future we will need a non-binary cost. About the second one, it is already implemented like this :)

if ( elevation_data(gridmap_index(0), gridmap_index(1)) > height_treshold_ ) // If point too high, it could be an obstacle
{
if (!has_edges_layer) {
ROS_WARN("No edges layer found !!");
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If this check fails, the warning will be printed many times. Can you do the check outside the for loop?

virtual void setupDynamicReconfigure(ros::NodeHandle& nh);


int combination_method_;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You could use an enum or an enum class for this.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't think it is possible to get an enum type from a yaml file, is it?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't think so, but you could use a string that you transform to an enum. This makes it easier for the user to tweak the parameters of your layer (0, 1 or 2 is not very descriptive)

rolling_window_ = layered_costmap_->isRolling();

ElevationLayer::matchSize();
current_ = true;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Value of current_ is used by move_base to know if the layer is up-to-date (look here). Therefore it should be updated depending on elevation layer updates. For example if we have not received elevation map for certain period we should make this value to false so that move_base gives a warning and stops the robot.

filters_configuration_loaded_ = false;
global_frame_ = layered_costmap_->getGlobalFrameID();
elevation_layer_name_ = "elevation";
edges_layer_name_ = "edges";
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Since both elevation_layer_name_ and edges_layer_name_ come from elevation mapping, shouldn't the names be configurable using parameters?

current_ = true;
elevation_map_received_ = false;
filters_configuration_loaded_ = false;
global_frame_ = layered_costmap_->getGlobalFrameID();
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

global_frame_ is never used in the code!

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

AFAIK, the elevation layer can only work if the costmap frame is set to odom (or same as the frame used by elevation mapping). The user should be warned if the frames in costmap and elevation layers are not matching.

{
std::lock_guard<std::mutex> lock(elevation_map_mutex_);
elevation_map_ = filtered_map;
height_treshold_ /= 2; // Half the treshold since the highest sharpness is at midheigth of the obstacles
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

threshold (instead of treshold) is official spelling in English :)

Copy link
Member

@pfankhauser pfankhauser left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks, but this PR is not close to be merged. No documentation, wrong formatting, and some open issues from other reviewers. Please work on this before I take another look.

@@ -0,0 +1,3 @@
# Elevation layer
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Don't add README files for each package, but please document the new software in the main README.

input_layer: elevation
output_layer: edges
expression: 'sumOfFinites([0,1,0;1,-4,1;0,1,0].*elevation)' # Edge detection.
# expression: 'sumOfFinites([0,-1,0;-1,5,-1;0,-1,0].*elevation)' # Sharpen.
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Delete unused expressions.

@@ -0,0 +1,12 @@
elevation_filters:
# Edge detection on elevation layer with convolution filter as alternative to filter above.
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This comment makes no sense because there's no filter above?

expression: 'sumOfFinites([0,1,0;1,-4,1;0,1,0].*elevation)' # Edge detection.
# expression: 'sumOfFinites([0,-1,0;-1,5,-1;0,-1,0].*elevation)' # Sharpen.
compute_empty_cells: false
edge_handling: mean # options: inside, crop, empty, mean
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The comments here are instructions for the grid map filter example, not fitting here.


#pragma once

#include <atomic>
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please group and order the includes correctly, see coding guidelines.

#include <filters/filter_chain.h>
#include <message_filters/subscriber.h>
#include <ros/ros.h>
#include "grid_map_ros/GridMapRosConverter.hpp"
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Use correct < and "

*/
void onInitialize() override;

/**
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Everywhere: Use correct doxygen formatting!

}

void ElevationLayer::onInitialize() {
nh_ = ros::NodeHandle("~/" + name_);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

No abbreviations! See programming guidelines.


ElevationLayer::matchSize();
current_ = true;
elevation_map_received_ = false;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Everywhere: Use correct formatting for variables.

@pfankhauser pfankhauser self-requested a review December 4, 2018 13:26
@pfankhauser
Copy link
Member

I don't think elevation_layer is a good name for this package. Since it's a plugin for costmap_2d, this should be somehow clear. What are the names of other costmap_2d plugins?

@chisarie
Copy link
Author

chisarie commented Dec 4, 2018

I don't think elevation_layer is a good name for this package. Since it's a plugin for costmap_2d, this should be somehow clear. What are the names of other costmap_2d plugins?

The other names are inflation_layer, obstacle_layer, static_layer, voxel_layer.
https://github.com/ANYbotics/navigation_ros/tree/master/costmap_2d/plugins

@chisarie
Copy link
Author

@pfankhauser Hi, addressed the comments you wrote, i hope i didn't miss anything.


This is a plugin for [costmap_2d] and provides an implementation of a 2d costmap. With the `elevation_mapping_costmap_2d_plugin` package, the generated elevation map can be used to build a costmap_2d to work with the [ROS] [navigation] stack. It takes the elevation map as input and generates a layer of obstacles in the local costmap_2d.

The layer applies a height and a sharpness filter to the incoming data. The behaviour can be adjusted tuning the parameters `heightThreshold_` and `edgesSharpnessThreshold_`. The former labels as obstacles all data above a certain height, while the latter labels as obstacles data above a certain edge sharpness, where a sharp edge is defined as a sudden change in height; this is used to avoid labeling slopes as obstacle.
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

heightThreshold_ is this an absolute height or relative to the neighbor cells?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Absolute

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

So is this really sensible when the robot's position drifts over time?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We did't experience that. I mean, even if the robot drifts in map frame, the sensor still outputs proper depth values and elevation mapping is built on those, irrespective of the drift (right? I am not that sure anymore, i should doublecheck)

@@ -235,6 +235,11 @@ This is the main Robot-Centric Elevation Mapping node. It uses the distance sens

The data for the sensor noise model.

### elevation_mapping_costmap_2d_plugin

This is a plugin for [costmap_2d] and provides an implementation of a 2d costmap. With the `elevation_mapping_costmap_2d_plugin` package, the generated elevation map can be used to build a costmap_2d to work with the [ROS] [navigation] stack. It takes the elevation map as input and generates a layer of obstacles in the local costmap_2d.
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Whats the message type of costmap_2d, OccupancyGrid? It would be good to state this.

globalFrame_ = layered_costmap_->getGlobalFrameID();

// get parameters from config file
if (!nodeHandle_.param("elevation_topic", elevationTopic_, std::string(""))) {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

All these parameters are not described in the README?

enum CombinationMethod { Overwrite, Maximum, Unknown };

/*!
* converts the string from the yaml file to the proper CombinationMethod enum
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

All comments should start with a capital letter and end with a period, see coding guidelines.

@chisarie
Copy link
Author

chisarie commented Mar 11, 2019

@harmishhk @hogabrie @pfankhauser Since we will not use costmap_2d, this is not needed anymore, right? Or Peter, do you want it merged anyway? Otherwise it can be closed.

@harmishhk
Copy link
Contributor

We will not use costmap_2d in the future, but we are using at the moment, and I also think this could be useful for community if we merge. Moreover, the functionality of filtering at specific height will be needed also with grid_map based cost-map that we are going to use in the future.

}

void ElevationLayer::setupDynamicReconfigure(ros::NodeHandle& nodeHandle_) {
dsrv_.reset(new dynamic_reconfigure::Server<elevation_mapping_costmap_2d_plugin::ElevationPluginConfig>(nodeHandle_));
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please refrain from using abbreviations.


void ElevationLayer::setupDynamicReconfigure(ros::NodeHandle& nodeHandle_) {
dsrv_.reset(new dynamic_reconfigure::Server<elevation_mapping_costmap_2d_plugin::ElevationPluginConfig>(nodeHandle_));
dynamic_reconfigure::Server<elevation_mapping_costmap_2d_plugin::ElevationPluginConfig>::CallbackType cb =
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Same here, please refrain from using abbreviations.

void ElevationLayer::reset() {
deactivate();
resetMaps();
current_ = true;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Current what? Please use clearer variable names.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

All those variables are derived from base classes in costmap_2d .....

@maximilianwulf
Copy link
Contributor

@harmishhk and @pfankhauser what should we do about this PR?
Is @chisarie working on the changes?

@pfankhauser
Copy link
Member

@harmishhk and @pfankhauser what should we do about this PR?
Is @chisarie working on the changes?

@chisarie is not working on this any more. There are some changes needed for this PR before we can merge it, as long as nobody needs it, I suggest to leave it in this branch for later reference.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

6 participants